//----------------------------------------------------------------------------------------------
//	Filename:	PointCloud.cpp
//	Author:		Sandro Spina
//	Date:		27/07/2012
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
#include "Shape/KDTreePointCloud.h"

#include <boost/shared_ptr.hpp>
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/lexical_cast.hpp>

#include "Maths/Random.h"
#include "Maths/Montecarlo.h"
#include "Material/MaterialGroup.h"

using namespace Illumina::Core;

//----------------------------------------------------------------------------------------------
#define MAX_STACK_NODES	64
//----------------------------------------------------------------------------------------------
namespace Illumina 
{
	namespace Core
	{
		//----------------------------------------------------------------------------------------------
		// Stack Element for stack-based traversal of KD-Tree
		//----------------------------------------------------------------------------------------------
		struct KDTreeStackElement
		{
		public:
			float Min;
			float Max;
			KDTreePointCloudNode *pNode;

			KDTreeStackElement(void) { }

			KDTreeStackElement(KDTreePointCloudNode *p_pNode, float p_fMin, float p_fMax)
				: Min(p_fMin)
				, Max(p_fMax)
				, pNode(p_pNode)
			{ }

			KDTreeStackElement(const KDTreeStackElement &p_stackElement)
				: Min(p_stackElement.Min)
				, Max(p_stackElement.Max)
				, pNode(p_stackElement.pNode)
			{ }
		};
	}
}


//----------------------------------------------------------------------------------------------
// Helper functions for allocation and deallocation of KDTree nodes
//----------------------------------------------------------------------------------------------
KDTreePointCloudNode* KDTreePointCloud::RequestNode(void)
{
	return new KDTreePointCloudNode();
}
//----------------------------------------------------------------------------------------------
int KDTreePointCloud::ReleaseNode(KDTreePointCloudNode *p_pNode)
{
	int nodesFreed = 0;

	if (p_pNode != NULL && p_pNode->Type == KDTreePointCloud::Internal)
	{
		nodesFreed += ReleaseNode(p_pNode->m_pChild[0]);
		nodesFreed += ReleaseNode(p_pNode->m_pChild[1]);
	}
	else
	{
		Safe_Delete(p_pNode);
		nodesFreed++;
	}

	return nodesFreed;
}


//----------------------------------------------------------------------------------------------
// Constructors and destructor
//----------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------
KDTreePointCloud::KDTreePointCloud(int p_nMaxObjectsPerLeaf, int p_nMaxTreeDepth)
	: ITreePointCloud()
	, m_nMaxLeafObjects(p_nMaxObjectsPerLeaf)
	, m_nMaxTreeDepth(p_nMaxTreeDepth)	
{	
	m_noise = 0.f;		
}
//----------------------------------------------------------------------------------------------
KDTreePointCloud::KDTreePointCloud(const std::string &p_strName, int p_nMaxObjectsPerLeaf, int p_nMaxTreeDepth)
	: ITreePointCloud(p_strName)
	, m_nMaxLeafObjects(p_nMaxObjectsPerLeaf)
	, m_nMaxTreeDepth(p_nMaxTreeDepth)	
{ 	
	m_noise = 0.f;
}
//----------------------------------------------------------------------------------------------
KDTreePointCloud::~KDTreePointCloud(void)
{}

bool KDTreePointCloud::Compile(void) 
{ 
	//Build kdtree over these points for rendering
	BuildHierarchy(&m_rootNode, m_pointList, 0, 0);	
	return true; 
}

//----------------------------------------------------------------------------------------------
int KDTreePointCloud::DiscardPointsWithType(int p_type, int p_rangeMin, int p_rangeMax) {	
	std::vector<int> toRemove;	
	std::stringstream mess;
	std::vector<CloudPoint> m_pointList_new;

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_typeId != p_type)
		{
			m_pointList_new.push_back(m_pointList[i]);
		}
	}

	int count = m_pointList.size() - m_pointList_new.size();

	m_pointList.clear();
	m_pointList = m_pointList_new;
	
	mess.str(std::string("")); mess << "[KDTreePointCloud] Removed " << count << " points with Type=" << p_type;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);
		
	return m_pointList.size();
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::DiscardPointsWithSegmentId(int p_segment, int p_rangeMin, int p_rangeMax) {
	std::stringstream mess;		
	std::vector<int> toRemove;	

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_segmentId == p_segment)
		{
			toRemove.push_back(i);
		}
	}
	
	std::vector<CloudPoint>::iterator piter = m_pointList.begin();
	int count = 0;

	for (int i = toRemove.size()-1; i>-1; i--)
	{
		m_pointList.erase(piter+toRemove[i]);
		count++;
	}

	mess.str(std::string("")); mess << "[KDTreePointCloud] Removed " << count << " points with Segment ID " << p_segment;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);
	
	return true;
}

//----------------------------------------------------------------------------------------------
void KDTreePointCloud::SetAllPointsInOneSegment() {	
	std::stringstream mess;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();
	m_pointListSegmentIdsCache.clear();

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{		
		m_pointListSegmentIdsCache.push_back(m_pointList[i].m_segmentId);
		m_pointList[i].m_visible=1;
		m_pointList[i].m_segmentId=1;
	}	

	mess.str(std::string("")); mess << "[KDTreePointCloud] Setting to Segment ID 1";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);
}

//----------------------------------------------------------------------------------------------
void KDTreePointCloud::RestoreAllPointsSegmentIdsFromCache() {	
	std::stringstream mess;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();	

	for (int i=0; i<m_pointListSegmentIdsCache.size(); i++)
	{				
		m_pointList[i].m_segmentId=m_pointListSegmentIdsCache[i];
	}	

	mess.str(std::string("")); mess << "[KDTreePointCloud] Segment Ids Restored from Cache";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetInvisibleAllPoints() {	
	std::stringstream mess;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{		
		m_pointList[i].m_visible=0;		
	}	

	mess.str(std::string("")); mess << "[KDTreePointCloud] Hide ALL - Set To Invisible " << p_rangeMax - p_rangeMin << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);
	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetInvisibleAllPointsExceptSeeds() {		
	std::stringstream mess;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();
	int seeds = 0;

	mess << "[KDTreePointCloud] Setting to Visible ... ";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Info);

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{		
		if (m_pointList[i].m_seed==1)
		{
			m_pointList[i].m_visible=1;
			seeds++;
		}
		else 
		{
			m_pointList[i].m_visible=0;
		}

	}

	mess.str(std::string("")); mess << "[KDTreePointCloud] " << seeds << " Seed Points";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetVisibleAllPoints() {
	std::stringstream mess;

	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{		
			m_pointList[i].m_visible=1;		
	}	

	mess.str(std::string("")); mess << "[KDTreePointCloud] Show ALL - Set To Visible " << p_rangeMax - p_rangeMin << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetVisibleAllPointsExceptForSegmentId(int p_segment) {	
	std::stringstream mess; 
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();
	int count = 0;

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_segmentId == p_segment)
		{
			m_pointList[i].m_visible=0;
		}
		else
		{
			m_pointList[i].m_visible=1;
			count++;
		}
	}	

	mess.str(std::string("")); mess << "[KDTreePointCloud] Showing all except segment "<< p_segment << ". Set to visible " << count << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetVisibleNextPrimitiveInSegment(int p_segmentId, int p_internalSegmentId)
{	 	
	std::stringstream mess;

	int count = 0;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_segmentId == p_segmentId && m_pointList[i].m_segmentId2 == p_internalSegmentId)
		{
			m_pointList[i].m_visible=1;			
			count++;
		}
		else
		{
			m_pointList[i].m_visible=0;			
		}
	}

	mess.str(std::string("")); mess << "[KDTreePointCloud] Show Segment " << p_segmentId << " internal idx::" << p_internalSegmentId << "Set to visible " << count << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetVisibleNonFittedPointsInSegment(int p_segmentId)
{	 	
	std::stringstream mess; 

	int count = 0;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_segmentId == p_segmentId && m_pointList[i].m_segmentId2 == -1)
		{
			m_pointList[i].m_visible=1;			
			count++;
		}
		else
		{
			m_pointList[i].m_visible=0;			
		}
	}

	mess.str(std::string("")); mess << "[KDTreePointCloud] Show Non Fitted from Segment " << p_segmentId << ". Set to visible " << count << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetInvisibleAllPointsExceptForSegmentId(int p_segment) {
	std::stringstream mess;

	int count = 0;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();
	int type_id = 0;

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_segmentId == p_segment)
		{
			m_pointList[i].m_visible=1;			
			//type_id = m_pointList[i].m_typeId;
			count++;
		}
		else
		{
			m_pointList[i].m_visible=0;			
		}
	}

	mess.str(std::string("")); mess << "[KDTreePointCloud] Set to visible " << count << " points from seg#" << p_segment<< " only.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetPointInvisible(int p_point) {

	std::stringstream mess; mess << "[KDTreePointCloud] Hiding Point at index " << p_point;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Info);
	
	m_pointList[p_point].m_visible = 0;
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetPointVisible(int p_point) {
	std::stringstream mess; mess << "[KDTreePointCloud] Showing Point at index " << p_point;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Info);
	
	m_pointList[p_point].m_visible = 1;
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetInvisiblePointsWithSegmentId(int p_segment) {
	std::stringstream mess; 
	
	int count = 0;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_segmentId == p_segment)
		{
			m_pointList[i].m_visible=0;
			count++;
		}
	}	

	mess.str(std::string("")); mess << "[KDTreePointCloud] Hiding segment " << p_segment << ". Set to invisible " << count << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetPointNeighbourhoodVisible(int p_point) {
	std::stringstream mess; 
	mess << "[KDTreePointCloud] Showing Neighbour Points #idx=" << p_point << ", Pos=" << m_pointList[p_point].m_vertex.Position.ToString() << ", SegId=" << m_pointList[p_point].m_segmentId;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Info);
	 
	int count = 0;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();

	SetInvisibleAllPoints();

	//If map is empty populate it with references to all points
	if (m_allPointsMap.empty()) for (int i = p_rangeMin; i<p_rangeMax; i++) m_allPointsMap[i] = &(m_pointList[i]);
	
	//Build kdtree over these points for kNN (will eventually merge this kd-tree with the one for rendering)
	typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<float, KDTreePointCloud>, KDTreePointCloud, 3 /* dim */> my_kd_tree_t;
	my_kd_tree_t index(3 /*dim*/, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
	index.buildIndex();		

	const size_t num_results = GetkNNType();
	size_t *ret_index = new size_t[num_results];
	float *out_dist_sqr = new float[num_results];
	nanoflann::KNNResultSet<float> resultSet(num_results);
	
	Eigen::Matrix3f covariance = Eigen::Matrix3f();				
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;

	float query_pt[3] = { m_pointList[p_point].m_vertex.Position[0], m_pointList[p_point].m_vertex.Position[1], m_pointList[p_point].m_vertex.Position[2]};				
	resultSet.init(&ret_index[0], &out_dist_sqr[0] );			
	index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));	
	
	for (int j=0; j<num_results; j++) m_pointList[ret_index[j]].m_visible = true;
	
	mess.str(std::string("")); mess << "[KDTreePointCloud] Set to visible " << count << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetVisiblePointsWithSegmentId(int p_segment) {	
	std::stringstream mess;

	int count = 0;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();

	int typeId = 0;

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_segmentId == p_segment)
		{
			m_pointList[i].m_visible=1;
			typeId = m_pointList[i].m_typeId;
			count++;
		}
	}
		
	mess.str(std::string("")); mess << "[KDTreePointCloud] Showing Segment " << p_segment << ". Set to visible " << count << " points from seg#" << p_segment << ". Type ID=" << typeId;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetVisiblePointsWithSegmentIds(std::vector<int> p_segments)
{
	SetInvisibleAllPoints();
	for (int i = 0; i<p_segments.size(); i++)
	{
		SetVisiblePointsWithSegmentId(p_segments[i]);
	}
	return true;
}

bool KDTreePointCloud::AddToVisiblePointsWithSegmentIds(std::vector<int> p_segments)
{	
	for (int i = 0; i<p_segments.size(); i++)
	{
		SetVisiblePointsWithSegmentId(p_segments[i]);
	}
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetVisiblePointsWithTypeId(int p_type) {
	std::stringstream mess;
	
	int count = 0;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();
	
	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_typeId == p_type)
		{
			m_pointList[i].m_visible=1;
			count++;	
		} else { m_pointList[i].m_visible=0; }
	}
		
	mess.str(std::string("")); mess << "[KDTreePointCloud] Show Pnts with Type=" << p_type << ". Set to visible " << count << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------

bool KDTreePointCloud::SetVisiblePointsWithTypeId(int p_type, std::map<int,int> &segments_map) {
	std::stringstream mess;
	
	segments_map.clear();
	int count = 0;
	int p_rangeMin = 0;
	int p_rangeMax = m_pointList.size();	
	std::map<int, int>::iterator segments_map_it;
	int prev_seg_id = -1;
	int crt_seg_id = 0;

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_typeId == p_type)
		{
			m_pointList[i].m_visible=1;
			count++;
			crt_seg_id = m_pointList[i].m_segmentId;
			if (prev_seg_id != crt_seg_id)
			{
				segments_map_it = segments_map.find(m_pointList[i].m_segmentId);
				if (segments_map_it == segments_map.end()) 
				{
					segments_map[m_pointList[i].m_segmentId] = 0;
					segments_map_it = segments_map.find(m_pointList[i].m_segmentId);
				}
			}
			prev_seg_id = crt_seg_id;
			(*segments_map_it).second++;
		} else { m_pointList[i].m_visible=0; }
	}			

	mess.str(std::string("")); mess << "[KDTreePointCloud] Show Pnts with Type=" << p_type << ". Set to visible " << count << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::SetVisiblePointsWithIds(std::vector<int> &p_indices) {
	std::stringstream mess; mess << "[KDTreePointCloud] Set to Visible " <<  p_indices.size() << " Edge Points .. ";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Info);
	
	for (int i = 0; i<p_indices.size(); i++)
	{
		m_edgePointsMap.at(p_indices[i])->m_visible=true;
	}
	p_indices.clear();	
	std::cin.ignore();
	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::DiscardAllButPointsWithSegmentId(int p_segment, int p_rangeMin, int p_rangeMax) {		
	std::stringstream mess; mess << "[KDTreePointCloud] Discarding Points NOT with Segment ID " << p_segment;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Info);

	std::vector<int> toRemove;	

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		if (m_pointList[i].m_typeId != p_segment)
		{
			toRemove.push_back(i);
		}
	}
	
	std::vector<CloudPoint>::iterator piter = m_pointList.begin();
	int count = 0;

	for (int i = toRemove.size()-1; i>-1; i--)
	{
		m_pointList.erase(piter+toRemove[i]);
		count++;
	}

	mess.str(std::string("")); mess << "[KDTreePointCloud] Removed " << count << " points.";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Warning);	
	return true;
}

//-------------------------------------------------------------------------------------------
bool KDTreePointCloud::AddNoise( float p_noise, int p_rangeMin, int p_rangeMax) {
	std::stringstream mess; mess << "[KDTreePointCloud] Applying Noise Function : " << p_noise;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Info);
	
	Vector3 noise_dir(0,0,0);
	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		noise_dir.X = (float)rand()/(float)RAND_MAX;
		noise_dir.Y = (float)rand()/(float)RAND_MAX;
		noise_dir.Z = (float)rand()/(float)RAND_MAX;
		noise_dir.Normalize();		
		m_pointList[i].m_vertex.Position += (noise_dir * p_noise);
	}
	return true;
}

//----------------------------------------------------------------------------------------------
// Method for updating point types ranging from rangeMin and rangeMax
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::ComputePointTypes(int p_rangeMin, int p_rangeMax) {
	//Build kdtree over these points for kNN (will eventually merge this kd-tree with the one for rendering)
	typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<float, KDTreePointCloud>, KDTreePointCloud, 3 /* dim */> my_kd_tree_t;
	my_kd_tree_t index(3 /*dim*/, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
	index.buildIndex();	
	
	const size_t num_results = GetkNNType();
	size_t *ret_index = new size_t[num_results];
	float *out_dist_sqr = new float[num_results];
	Eigen::Matrix3f covariance = Eigen::Matrix3f();
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
	float epsilon = 0.00004f;
	int pointsOnLine = 0;
	int pointsOnCorner = 0;

	int pointsOnEdge = 0;
	int pointsOnSurface = 0;
	int within_maxdist = 0;
	int m_type_id_0 = 0;

	float max_dist = GetkNNTypeRadius();

	std::stringstream mess; mess << "[ComputePointTypes] #Pnts=" << p_rangeMax-p_rangeMin << " kNNType=" << num_results << ", C=" << m_surface_curvature << ", W= " << m_world_type << ", R=" << max_dist;
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Error);

	nanoflann::KNNResultSet<float> resultSet(num_results);

	std::vector<int> edge_point_indices;
	
	const size_t num_results_temp = GetkNNType()/3;
	size_t *ret_index_temp = new size_t[num_results_temp];
	float *out_dist_sqr_temp = new float[num_results_temp];
	nanoflann::KNNResultSet<float> resultSetTemp(num_results_temp);
	std::map<int, float> temp_neighbours_map;
	std::vector<int> temp_neighbours_list;
	std::vector<float> temp_neighbours_dst_list;
	std::vector<int> dup_neighbours_list;

	float avg_n_count = 0;

	for (int i = p_rangeMin; i<p_rangeMax; i++)
	{
		int neighbours_count = num_results;
		if (m_pointList[i].m_typeId==-1) continue; //point was tagged as duplicate.		
		 
		if (m_kNNTypeMode==1)
		{
			//Get neighbours of current (closer) neighbour points implementation
			temp_neighbours_list.clear();
			temp_neighbours_map.clear();		
			dup_neighbours_list.clear();
			float query_pt[3] = { m_pointList[i].m_vertex.Position[0], m_pointList[i].m_vertex.Position[1], m_pointList[i].m_vertex.Position[2]};				
			resultSetTemp.init(&ret_index_temp[0], &out_dist_sqr_temp[0] );
			index.findNeighbors(resultSetTemp, &query_pt[0], nanoflann::SearchParams(10));	
			within_maxdist = 0;
			temp_neighbours_map[i] = 0.f;
			for (int i=1; i<num_results_temp; i++) 
			{
				if (out_dist_sqr_temp[i] != 0 && out_dist_sqr_temp[i] < max_dist) 
				{
					temp_neighbours_list.push_back(ret_index_temp[i]);
					temp_neighbours_dst_list.push_back(out_dist_sqr_temp[i]);
					temp_neighbours_map[ret_index_temp[i]] = out_dist_sqr_temp[i];
				} 
				else  
				{
					dup_neighbours_list.push_back(ret_index_temp[i]);
				}

			}
			for (int i=0; i<temp_neighbours_list.size(); i++) 
			{
				//temp_neighbours_map[temp_neighbours_list.at(i)] = temp_neighbours_dst_list.at(i);
				float query_pt[3] = { m_pointList[temp_neighbours_list.at(i)].m_vertex.Position[0], 
									m_pointList[temp_neighbours_list.at(i)].m_vertex.Position[1], 
									m_pointList[temp_neighbours_list.at(i)].m_vertex.Position[2]};
				//std::cout << "Checking point at idx " << temp_neighbours_list.at(i);
				resultSetTemp.init(&ret_index_temp[0], &out_dist_sqr_temp[0]);
				index.findNeighbors(resultSetTemp, &query_pt[0], nanoflann::SearchParams(10));
				for (int j=0; j<num_results_temp; j++)
				{
					//std::cout << "Neighbour idx = " << ret_index_temp[j] << std::endl;
					if (temp_neighbours_map.find(ret_index_temp[j]) == temp_neighbours_map.end()) 
					{
						temp_neighbours_map[ret_index_temp[j]] = out_dist_sqr_temp[j]; //this is a value > 0
					}
				}
			}
			int n_count = 0;
			for (int i=0; i<dup_neighbours_list.size(); i++) 
			{
				ret_index[n_count] = dup_neighbours_list[i]; //Copy point on same position.
				out_dist_sqr[n_count] = 0.f;
			}
			for (std::map<int, float>::iterator ni = temp_neighbours_map.begin(); ni != temp_neighbours_map.end(); ni++)
			{
				if (n_count >= num_results) 
				{
					neighbours_count = num_results;	
					break;
				} 
				else
				{
					ret_index[n_count] = (*ni).first;
					out_dist_sqr[n_count] = (*ni).second;
					n_count++;
				}
				neighbours_count = temp_neighbours_map.size();
			}
						
			//std::cout << "NC=" << neighbours_count << std::endl;
			avg_n_count += neighbours_count;
		}
		else 
		{
			//Default Method
			//Just get the neighbours of this point implementation			
			float query_pt[3] = { m_pointList[i].m_vertex.Position[0], m_pointList[i].m_vertex.Position[1], m_pointList[i].m_vertex.Position[2]};				
			resultSet.init(&ret_index[0], &out_dist_sqr[0] );
			index.findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));		
			within_maxdist = 0;					
		}		

		//Determine the density within this point by summing the distances to each neighbour
		//The smaller the distance the higher the density and our confidence in the result

		//*************
		//Compute covariance matrix over these neighbour points
		float c00, c11, c22, c01, c02, c12;
		c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
		
		//First Get Median Vector
		Vector3 median = Vector3(0);
								
		for (int j=0; j<neighbours_count; j++)
		{
			if (out_dist_sqr[j] < max_dist)
			{
				if (out_dist_sqr[j] < epsilon)
				{
					if (m_pointList[ret_index[j]].m_typeId!=-1) m_pointList[ret_index[j]].m_typeId = -1;  //point should be discarded.
				}
				else
				{
					median.X += m_pointList[ret_index[j]].m_vertex.Position[0];
					median.Y += m_pointList[ret_index[j]].m_vertex.Position[1];
					median.Z += m_pointList[ret_index[j]].m_vertex.Position[2];
					within_maxdist++;
				}
			}
		}
		
		if (within_maxdist < 3) { m_pointList[i].m_typeId = 0; m_type_id_0++; continue; } //we need at least three point to define a surface. skip this point and mark it as undefined

		median /= within_maxdist;

		//Now calculate diagonal of covariance matrix
		for (int j=0; j<neighbours_count; j++) 
		{
			if (out_dist_sqr[j] < max_dist)
			{
				c00 += (m_pointList[ret_index[j]].m_vertex.Position[0] - median.X) * (m_pointList[ret_index[j]].m_vertex.Position[0] - median.X);
				c11 += (m_pointList[ret_index[j]].m_vertex.Position[1] - median.Y) * (m_pointList[ret_index[j]].m_vertex.Position[1] - median.Y);
				c22 += (m_pointList[ret_index[j]].m_vertex.Position[2] - median.Z) * (m_pointList[ret_index[j]].m_vertex.Position[2] - median.Z);
				c01 += (m_pointList[ret_index[j]].m_vertex.Position[0] - median.X) * (m_pointList[ret_index[j]].m_vertex.Position[1] - median.Y);
				c02 += (m_pointList[ret_index[j]].m_vertex.Position[0] - median.X) * (m_pointList[ret_index[j]].m_vertex.Position[2] - median.Z);
				c12 += (m_pointList[ret_index[j]].m_vertex.Position[1] - median.Y) * (m_pointList[ret_index[j]].m_vertex.Position[2] - median.Z);
			}
		}				

		covariance << c00 / within_maxdist, c01 / within_maxdist, c02 / within_maxdist, 
						c01 / within_maxdist, c11 / within_maxdist, c12 / within_maxdist, 
						c02 / within_maxdist, c12 / within_maxdist, c22 / within_maxdist;
				
		eigensolver.compute(covariance);
		
		//Determine point type for m_pointList[i]
		if (m_world_type==0)
		{
			if (eigensolver.eigenvalues()[0] + eigensolver.eigenvalues()[1] < epsilon)
			{
				//this is a point on a line
				m_pointList[i].m_typeId = 1;
				pointsOnLine++;
			}
			else
			{
				//this is a point on a corner
				m_pointList[i].m_typeId = 2;
				pointsOnCorner++;
			}
		}

		if (m_world_type==1) //Normal planar 3D worlds
		{	
			m_pointList[i].m_normal.X = eigensolver.eigenvectors().col(0)[0];
			m_pointList[i].m_normal.Y = eigensolver.eigenvectors().col(0)[1];
			m_pointList[i].m_normal.Z = eigensolver.eigenvectors().col(0)[2];			
			if (eigensolver.eigenvalues()[0] * m_surface_curvature < eigensolver.eigenvalues()[1]) 
			{
				//this is a point on a surface
				m_pointList[i].m_typeId = 4;
				pointsOnSurface++;
			}
			else 
			{
				//this is a point on an edge
				m_pointList[i].m_typeId = 3;				
				pointsOnEdge++;
				//edge_point_indices.push_back(i);		//Indices will be fucked up after discarding those with type -1 !!!! 		
			}
		}
	}

	//std::cout << "Average N Count = " << avg_n_count / p_rangeMax << std::endl;	

	//Cleanup The Cloud from duplicate points.
	//std::cout << "[KDTreePointCloud] Cleaning and Recompiling KD-Tree" << std::endl;
	int c = DiscardPointsWithType(-1, 0, m_pointList.size());  //Discard points which were found twice.
	int d = DiscardPointsWithType(0, 0, m_pointList.size());  //Discard points with not enough neighbours.
	if (c+d > 0) 
	{ 
		mess.str(std::string("")); mess << "[ComputePointTypes] Rebuilding KD-Tree over point cloud.";
		(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Error);			
		Compile(); 
	}


	//TEMP --- 
	/*
	CloudPoint crtPoint(1,1,1);
	Vector3 crtSeed;
	std::vector<Vector3> seedPointsSurface;
	std::vector<Vector3> seedPointsEdge;
	bool addToSeeds = true;
	for (int i=0; i<m_pointList.size(); i++)
    {
		crtPoint = m_pointList.at(i);
		if (crtPoint.m_typeId == 4)
		{
			for (int j=0; j<seedPointsSurface.size(); j++)
			{
				crtSeed = seedPointsSurface.at(j);
				if (Vector3::DistanceSquared(crtSeed, crtPoint.m_vertex.Position) < 2.0f) { addToSeeds = false; break; }
			}
			if (addToSeeds) { seedPointsSurface.push_back(crtPoint.m_vertex.Position); }
			addToSeeds = true;
		}

		addToSeeds=true;
		if (crtPoint.m_typeId == 3)
		{
			for (int j=0; j<seedPointsEdge.size(); j++)
			{
				crtSeed = seedPointsEdge.at(j);
				if (Vector3::DistanceSquared(crtSeed, crtPoint.m_vertex.Position) < 0.5f) { addToSeeds = false; break; }
			}
			if (addToSeeds) { seedPointsEdge.push_back(crtPoint.m_vertex.Position); }
			addToSeeds = true;
		}

    }

	std::cout << "surface::" << seedPointsSurface.size() << " edge::" << seedPointsEdge.size() << std::endl;

	std::ofstream myfile_surface;
	std::ofstream myfile_edge;
	std::string _filepath_surface = "C:\\Data\\Development\\IlluminaPCS\\Resource\\PointCloud\\barber_surface.asc";
	std::string _filepath_edge = "C:\\Data\\Development\\IlluminaPCS\\Resource\\PointCloud\\barber_edge.asc";
	std::cout << "[Training] Writing " << GetSize() << " Points to ASCII File " << _filepath_surface << " and " << _filepath_edge;
	myfile_surface.open (_filepath_surface);
	myfile_edge.open (_filepath_edge);	
	for (int i = 0; i< seedPointsSurface.size(); i++) myfile_surface << seedPointsSurface.at(i).X << " " << seedPointsSurface.at(i).Y << " " << seedPointsSurface.at(i).Z << "\n";
	for (int i = 0; i< seedPointsEdge.size(); i++) myfile_edge << seedPointsEdge.at(i).X << " " << seedPointsEdge.at(i).Y << " " << seedPointsEdge.at(i).Z << "\n";
	myfile_surface.close();	
	myfile_edge.close();
	std::cout << " [PRINTING DONE]" << std::endl;
	*/
	//TEMP ---

	mess.str(std::string("")); mess << "[ComputePointTypes] SurfacePnts = " << pointsOnSurface << ". EdgePnts = " << pointsOnEdge << ". DiscardedPnts = " << m_type_id_0 << " [DONE]";
	(*(m_environment->GetLogger()))["PointCloud"]->Write(mess.str(), LL_Error);	

	//CORNER DETECTION PASS
	if (m_edgeCornerDetect == 1)
	{
		//*************-----------Second pass-----------------
		//*************-----------Refine points with edge type
		ComputeCornerTypes(edge_point_indices, false);
	}
		
	return true;
}

//----------------------------------------------------------------------------------------------
// Method for detecting corner points on edges
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::ComputeCornerTypes(std::vector<int> &edge_point_indices, bool p_view) 
{
	const size_t num_results = GetkNNType();	
	bool debug = false;
	int corners_detected = 0;
	int corner_points_count = 0;
	const size_t num_results_temp = GetkNNType()/4;
	std::vector<int> show;
	float epsilon = 0.0004f;	
	int within_maxdist = 0;	
	float max_dist = GetkNNTypeRadius() * GetkNNTypeRadius();
	std::vector<int> seeds_queue;
	std::vector<int> seeds;

	std::cout << "[ComputeCornerTypes] Determine corners using kType=" << num_results << ", R=" << max_dist << std::endl;	

	typedef nanoflann::KDTreeSingleIndexAdaptor<nanoflann::L2_Simple_Adaptor<float, KDTreePointCloud>, KDTreePointCloud, 3 /* dim */> my_kd_tree_t;
	my_kd_tree_t index(3 /*dim*/, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
	index.buildIndex();	

	//First set everything to invisible !!!
	if (p_view) SetInvisibleAllPoints();

	//If not already set, determine which are the edge points .... i.e. candidates for becoming corners	
	if (edge_point_indices.size() == 0)
	{
		int pc_size = m_pointList.size();
		for (int i = 0; i<pc_size; i++)
		{				
			if (m_pointList[i].m_typeId==3) 
			{
				edge_point_indices.push_back(i); 
				m_edgePointsMap[i] = &(m_pointList[i]);
			}
		}	
	}	
	const size_t num_results_seed = GetkNNType()/3;
	size_t *ret_index_seed = new size_t[num_results_seed];
	float *out_dist_sqr_seed = new float[num_results_seed];
	nanoflann::KNNResultSet<float> resultSetSeed(num_results_seed);

	std::cout << "[ComputeCornerTypes] Selecting Seed Candidates from " << edge_point_indices.size() << " points. [Density= " << num_results_seed << "]" << std::endl;	
	float query_point[3];
	int edge_density = 0;
	int surface_density = 0;
	int corner_seed_id = 6;
	const Vector3 shift(100000.f,0.f,0.f);	
	
	for (int i=0; i<edge_point_indices.size(); i++)
	{
		query_point[0] = m_pointList[edge_point_indices[i]].m_vertex.Position[0]; 
		query_point[1] = m_pointList[edge_point_indices[i]].m_vertex.Position[1];
		query_point[2] = m_pointList[edge_point_indices[i]].m_vertex.Position[2];
		resultSetSeed.init(&ret_index_seed[0], &out_dist_sqr_seed[0] );
		index.findNeighbors(resultSetSeed, &query_point[0], nanoflann::SearchParams(10));
		edge_density = 0;
		surface_density = 0;
		for (int j=0; j<num_results_seed; j++) 
		{
			m_pointList[ret_index_seed[j]].m_typeId==3? edge_density++:surface_density++;
		}		
		//std::cout << "[" << m_pointList[edge_point_indices[i]].m_vertex.Position.ToString() << "] ED=" << edge_density << " SD=" << surface_density << std::endl; 
		//if (edge_density > surface_density+3) 
		if (edge_density == num_results_seed)
		{ 			
			m_pointList[edge_point_indices[i]].m_seed = 1;
			seeds.push_back(edge_point_indices[i]);
			//std::cout << "[" << edge_point_indices[i] << "] P=" << m_pointList[edge_point_indices[i]].m_vertex.Position.ToString() << std::endl;
			//show.push_back(edge_point_indices[i]); 
			//m_pointList[edge_point_indices[i]].m_typeId = corner_seed_id;  //this is only temporary until I compute the corners			
		}
	}
	std::cout << "[ComputeCornerTypes] " << seeds.size() << " seed points chosen." << std::endl;
	//return true; //Temporary

	//Once identified, move (as in translate in space to a far away location) seed points by shift amount. Reconstruct KDTree
	for (int i = 0; i<seeds.size(); i++) m_pointList[seeds[i]].m_vertex.Position += shift;
	my_kd_tree_t index2(3 /*dim*/, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10 /* max leaf */) );
	index2.buildIndex();		
	//Seed Selection and Shifting DONE*****

	//Corner Selection using PCA
	const size_t num_results_pca = GetkNNType();
	size_t *ret_index_pca = new size_t[num_results_pca];
	float *out_dist_sqr_pca = new float[num_results_pca];
	nanoflann::KNNResultSet<float> resultSetPca(num_results_pca);
	Eigen::Matrix3f covariance = Eigen::Matrix3f();				
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
	std::vector<int> corner_points_indices;
	int neighbour_seeds = 0;

	for (int i=0; i<seeds.size(); i++)
	{	
		float query_pt[3] = { m_pointList[seeds[i]].m_vertex.Position[0], 
							  m_pointList[seeds[i]].m_vertex.Position[1], 
							  m_pointList[seeds[i]].m_vertex.Position[2] };
		resultSetPca.init(&ret_index_pca[0], &out_dist_sqr_pca[0]);
		index2.findNeighbors(resultSetPca, &query_pt[0], nanoflann::SearchParams(10));
		
		//Compute covariance matrix over these neighbour points
		float c00, c11, c22, c01, c02, c12;
		c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
		
		//First Get Median Vector
		Vector3 median = Vector3(0);
		within_maxdist = 0;
		neighbour_seeds = 0;
		for (int j=0; j<num_results_pca; j++) 
		{
			median.X += m_pointList[ret_index_pca[j]].m_vertex.Position[0];
			median.Y += m_pointList[ret_index_pca[j]].m_vertex.Position[1];
			median.Z += m_pointList[ret_index_pca[j]].m_vertex.Position[2];			
			within_maxdist++;
		}
		
		if (within_maxdist < 3) { continue; } //not enough points to define a corner. Leave it as edge.

		median /= num_results_pca;

		//Now calculate diagonal of covariance matrix
		for (int j=0; j<within_maxdist; j++) 
		{
			c00 += (m_pointList[ret_index_pca[j]].m_vertex.Position[0] - median.X) * (m_pointList[ret_index_pca[j]].m_vertex.Position[0] - median.X);
			c11 += (m_pointList[ret_index_pca[j]].m_vertex.Position[1] - median.Y) * (m_pointList[ret_index_pca[j]].m_vertex.Position[1] - median.Y);
			c22 += (m_pointList[ret_index_pca[j]].m_vertex.Position[2] - median.Z) * (m_pointList[ret_index_pca[j]].m_vertex.Position[2] - median.Z);
			c01 += (m_pointList[ret_index_pca[j]].m_vertex.Position[0] - median.X) * (m_pointList[ret_index_pca[j]].m_vertex.Position[1] - median.Y);
			c02 += (m_pointList[ret_index_pca[j]].m_vertex.Position[0] - median.X) * (m_pointList[ret_index_pca[j]].m_vertex.Position[2] - median.Z);
			c12 += (m_pointList[ret_index_pca[j]].m_vertex.Position[1] - median.Y) * (m_pointList[ret_index_pca[j]].m_vertex.Position[2] - median.Z);
		}				

		covariance << c00 / within_maxdist, c01 / within_maxdist, c02 / within_maxdist, 
						c01 / within_maxdist, c11 / within_maxdist, c12 / within_maxdist, 
						c02 / within_maxdist, c12 / within_maxdist, c22 / within_maxdist;
				
		eigensolver.compute(covariance);
				
		//if ((eigensolver.eigenvalues()[0] + eigensolver.eigenvalues()[1]) < eigensolver.eigenvalues()[2])
		if (eigensolver.eigenvalues()[1] * 2 < eigensolver.eigenvalues()[2])
		{
			//this is a point on an edge
			//Do Nothing!!
			//std::cout<< "E [" << seeds[i] << "] P=" << (m_pointList[seeds[i]].m_vertex.Position - shift).ToString() << " N=" << within_maxdist << " E0=" << eigensolver.eigenvalues()[0] << " E1=" << eigensolver.eigenvalues()[1] << " E2=" << eigensolver.eigenvalues()[2] << std::endl; // Print the shifted back position.
		}
		else
		{
			//this is a point on a corner			
			//std::cout<< "[" << seeds[i] << "] N=" << within_maxdist << " E0=" << eigensolver.eigenvalues()[0] << " E1=" << eigensolver.eigenvalues()[1] << " E2=" << eigensolver.eigenvalues()[2] << std::endl;
			if (debug) std::cout<< "C [" << seeds[i] << "] P=" << (m_pointList[seeds[i]].m_vertex.Position - shift).ToString() << " N=" << within_maxdist << " E0=" << eigensolver.eigenvalues()[0] << " E1=" << eigensolver.eigenvalues()[1] << " E2=" << eigensolver.eigenvalues()[2] << std::endl; // Print the shifted back position.
			corner_points_indices.push_back(seeds[i]);
			//m_pointList[seeds[i]].m_typeId = 5; //Probably it will be a good idea to set all it's neighbours to corners.
			//for (int j=0; j<within_maxdist; j++) m_pointList[ret_index_pca[j]].m_typeId = 5;
		}
	}
	
	//Shift the seeds to their original position
	for (int i=0; i<seeds.size(); i++) m_pointList[seeds[i]].m_vertex.Position -= shift;

	//Re-build kd-tree index with seeds in their original position.
	index.buildIndex();

	//Finally iterator over the corner_points vector and set to corner type.
	std::cout << "[ComputeCornerTypes] PCA on seed points detected " << corner_points_indices.size() << " corner points" << std::endl;
	for (int i = 0; i<corner_points_indices.size(); i++)
	{		
		m_pointList[corner_points_indices[i]].m_typeId = 5; //Set this and all it's neighbours to corners.
		float query_pt[3] = { m_pointList[corner_points_indices[i]].m_vertex.Position[0], 
							  m_pointList[corner_points_indices[i]].m_vertex.Position[1], 
							  m_pointList[corner_points_indices[i]].m_vertex.Position[2] };
		resultSetPca.init(&ret_index_pca[0], &out_dist_sqr_pca[0]);
		index.findNeighbors(resultSetPca, &query_pt[0], nanoflann::SearchParams(10));
		for (int j=0; j<num_results_pca; j++) if (m_pointList[ret_index_pca[j]].m_typeId == 3) m_pointList[ret_index_pca[j]].m_typeId = 5;
	}	

	return true;

	//**************END Corner Detection using PCA**********
	//Start Corner Detection using ball rolling !!!! 

	//Now carry out corner detection		
	size_t *ret_index = new size_t[num_results*3];
	float *out_dist_sqr = new float[num_results*3];
	nanoflann::KNNResultSet<float> resultSet(num_results*3);

	size_t *ret_index_temp = new size_t[num_results_temp];
	float *out_dist_sqr_temp = new float[num_results_temp];
	nanoflann::KNNResultSet<float> resultSetTemp(num_results_temp);		
		
	std::map<int, float> temp_neighbours_map;
	std::vector<int> temp_neighbours_list;
	std::vector<float> temp_neighbours_dst_list;
	std::vector<int> dup_neighbours_list;

	float avg_n_count = 0;
	float sphere_radius = GetBallRadius();

	int edge_points_not_visited = edge_point_indices.size();
	std::map<int, bool> edge_visited; //if edge idx is in map then it has been visited

	ret_index = new size_t[num_results*3];
	out_dist_sqr = new float[num_results*3];
	nanoflann::KNNResultSet<float> resultSet_1(num_results*3);	
		
	std::vector<int> current_neighbours;
	std::vector<int> prev_neighbours;
	std::vector<Vector3> corner_points;
	std::vector<std::pair<float,std::vector<int>>> neighbourhoods;
	float crt_radius = 0.f;		

	bool use_prev = true;
	edge_points_not_visited = seeds.size();
	while (edge_points_not_visited > 0)
	{		
		current_neighbours.clear();
		prev_neighbours.clear();
		sphere_radius = GetBallRadius();
		use_prev=true;
		//Pick a seed ... Currently this is too arbitrary
		int new_seed_index = 0;
		for (int i=0; i<seeds.size(); i++)
		{
			if (edge_visited.find(seeds[i]) == edge_visited.end()) {
				//Start rolling the ball from this edge point
				float query_pt[3] = { m_pointList[seeds[i]].m_vertex.Position[0], 
									  m_pointList[seeds[i]].m_vertex.Position[1], 
									  m_pointList[seeds[i]].m_vertex.Position[2]};	
				resultSet_1.init(&ret_index[0], &out_dist_sqr[0] );
				index.findNeighbors(resultSet_1, &query_pt[0], nanoflann::SearchParams(10));
				edge_visited[seeds[i]] = true;
				show.push_back(seeds[i]);
				edge_points_not_visited--;
				new_seed_index = seeds[i];
				//if (debug) std::cout << "[" << edge_points_not_visited << "] Seeding from Idx=" << new_seed_index << " Pos= " + m_pointList[new_seed_index].m_vertex.Position.ToString() << std::endl;
				break;
			}
		}				
		//View the seed position
		if (p_view) SetVisiblePointsWithIds(show);		

		//Compute First Median Position
		Vector3 median = Vector3(0);
		int edge_neighbours = 0;
		int en = 0;
		int cn = 0;
		int furthest_point_idx = 0;
		//Result set stores the neighbours of the seed
		std::vector<int>crt_neighbourhood;
		for (int j=0; j<num_results*3; j++) 
		{
			if (m_pointList[ret_index[j]].m_typeId==corner_seed_id && (edge_visited.find(ret_index[j]) == edge_visited.end()))
			{
				median.X += m_pointList[ret_index[j]].m_vertex.Position[0];
				median.Y += m_pointList[ret_index[j]].m_vertex.Position[1];
				median.Z += m_pointList[ret_index[j]].m_vertex.Position[2];
				edge_visited[ret_index[j]] = true;
				show.push_back(ret_index[j]);
				crt_neighbourhood.push_back(ret_index[j]);
				edge_points_not_visited--;
				edge_neighbours++;
				furthest_point_idx = ret_index[j];
				//if (debug) std::cout << "Updating furthest point index to " << furthest_point_idx << " Distance:" << out_dist_sqr[j] << std::endl;
			}
			
			if (j<num_results && (m_pointList[ret_index[j]].m_typeId==3 
					|| m_pointList[ret_index[j]].m_typeId==corner_seed_id)) en++;   //take in consideration only those points which are very close
			if (j<num_results && m_pointList[ret_index[j]].m_typeId==5) cn++;	//take in consideration only those points which are very close

			if (out_dist_sqr[j] > sphere_radius) sphere_radius = out_dist_sqr[j];
			if (edge_neighbours == num_results) 
			{						
				break;
			}			
		}

		if (debug) 
		{
			std::cout << "[" << edge_points_not_visited << " Seeds To Process] This Seed Idx=" << new_seed_index << " Pos= " << m_pointList[new_seed_index].m_vertex.Position.ToString() << std::endl;		
			std::cout << "#Seed Neighbours: " << edge_neighbours << " Furthest Point Idx: " << furthest_point_idx << " Pos: " << m_pointList[furthest_point_idx].m_vertex.Position.ToString() << std::endl;
			std::cout << "Dir seed and furthest point: " << Vector3::Normalize(m_pointList[furthest_point_idx].m_vertex.Position -  m_pointList[new_seed_index].m_vertex.Position).ToString() << std::endl;
		}		

		if (edge_neighbours == 0) {
			//this is problematic ... SHOULD CHECK WHY THIS IS HAPPENING
			if (debug) std::cout << "[Seed is Isolated] #" << edge_points_not_visited << " - " << m_pointList[new_seed_index].m_vertex.Position.ToString()  
						<< " .. try another point!!" << std::endl;
			if (cn > en) 
			{
				m_pointList[new_seed_index].m_typeId = 5;
				if (debug) std::cout << "[Seed Surrounded by Corner Points] Changing type to corner" << std::endl;
			}
			continue;
		} 
		else 
		{
			//Update new radii values
			sphere_radius = sphere_radius * 3; //This will be kept constant ... and is bigger than optimal. why *4 ... BOQ!!				
			crt_radius = sphere_radius / 3;		
			if (debug) std::cout << "[KDTreePointCloud] Setting Ball Sp Rad (x3) to " << sphere_radius << " (tight) to " << crt_radius << std::endl;			
		}
		
		neighbourhoods.push_back(std::pair<float,std::vector<int>>(crt_radius,crt_neighbourhood));
		if (p_view) SetVisiblePointsWithIds(show);

		median /= edge_neighbours;

		Vector3 initial_neighbourhood_median(median);

		//compute set of neighbours around the furthest_point_idx
		std::vector<int>crt_neighbourhood_1;		
		float query_pt[3] = { m_pointList[furthest_point_idx].m_vertex.Position[0], m_pointList[furthest_point_idx].m_vertex.Position[1], m_pointList[furthest_point_idx].m_vertex.Position[2]};				
		resultSet_1.init(&ret_index[0], &out_dist_sqr[0] );
		index.findNeighbors(resultSet_1, &query_pt[0], nanoflann::SearchParams(10));
		edge_neighbours = 0;
		for (int j=0; j<num_results*3; j++)
		{
			if (m_pointList[ret_index[j]].m_typeId==corner_seed_id && (edge_visited.find(ret_index[j]) == edge_visited.end()))
			{
				current_neighbours.push_back(ret_index[j]);
				edge_visited[ret_index[j]] = true;
				show.push_back(ret_index[j]);
				crt_neighbourhood_1.push_back(ret_index[j]);		
				edge_points_not_visited--;
				edge_neighbours++;
			}
			if (edge_neighbours == num_results) break;
		}

		if (edge_neighbours == 0) { 
			if (debug) std::cout << "[Furtherest Point is Isolated] Zero neighbours for this point ... try another point!!" << std::endl;
			continue;    //Apparently we are at the end of the edge !!!
		}
		neighbourhoods.push_back(std::pair<float,std::vector<int>>(crt_radius,crt_neighbourhood_1));
		if (p_view) SetVisiblePointsWithIds(show);

		//fill prev neighbours vector with current neighbours initially.
		for (std::vector<int>::iterator ni = current_neighbours.begin(); ni != current_neighbours.end(); ni++)
		{
			prev_neighbours.push_back(*ni);
		}

		bool fit_within_sphere = true;
		//move along until a corner is encountered.

		if (debug) std::cout << "[" << edge_points_not_visited << "] Start Rolling From Point: " << m_pointList[furthest_point_idx].m_vertex.Position.ToString() << std::endl;
		int roll_count = 0;	
		while (fit_within_sphere)
		{
			Vector3 prev_median(median);
			median.X = 0; median.Y = 0; median.Z = 0;
			for (std::vector<int>::iterator ni = current_neighbours.begin(); ni != current_neighbours.end(); ni++)
			{
				median.X += m_pointList[*ni].m_vertex.Position[0];
				median.Y += m_pointList[*ni].m_vertex.Position[1];
				median.Z += m_pointList[*ni].m_vertex.Position[2];
			}

			median /= current_neighbours.size();

			if (debug) std::cout << "[#" << edge_points_not_visited << "] RM: " << median.ToString() << " Dir: " << Vector3::Normalize(median -  prev_median).ToString() << std::endl;			

			//check whether these fit in the 'rolling' sphere.
			float new_sphere_radius = 0.f;
			int do_not_fit = 0;
			float temp_sphere_radius = 0.f;
			float max_temp_sphere_radius = 0.f;
			for (std::vector<int>::iterator ni = current_neighbours.begin(); ni != current_neighbours.end(); ni++)
			{
				temp_sphere_radius = Vector3::DistanceSquared(m_pointList[*ni].m_vertex.Position, median);
				if (temp_sphere_radius > sphere_radius) 
				{
					//fit_within_sphere = false;
					do_not_fit++;
					new_sphere_radius = Vector3::DistanceSquared(m_pointList[*ni].m_vertex.Position, median);
				}
				if (temp_sphere_radius > max_temp_sphere_radius) max_temp_sphere_radius = temp_sphere_radius;
			}

			if (max_temp_sphere_radius > crt_radius)
			{
				crt_radius = max_temp_sphere_radius;
				if (debug) std::cout << "[Neighbourhood vector RETAINED] - Size=" << neighbourhoods.size() << " Radius=" << crt_radius << std::endl;
			} 
			else 
			{
				//clear all neighbourhoods we've seen so far excpet for the last one.
				if (neighbourhoods.size() > 1)
				{
					std::pair<float,std::vector<int>> nback = neighbourhoods.back();				
					neighbourhoods.clear();
					neighbourhoods.push_back(nback);
				}
				if (debug) std::cout << "[Neighbourhood vector CLEARED except for last element]" << std::endl;
			}


			if (do_not_fit > 4) fit_within_sphere = false; //4?? why ... 

			if (fit_within_sphere == true) 
			{
				//std::cout<<"All points fit within sphere radius of " << sphere_radius << std::endl;
				//replace prev with current neighbours
				roll_count++;
				use_prev=false;
				prev_neighbours.clear();
				for (std::vector<int>::iterator ni = current_neighbours.begin(); ni != current_neighbours.end(); ni++)
				{				
					prev_neighbours.push_back(*ni);
				}

				//get the next bunch of neighbours
				current_neighbours.clear();
				std::vector<int> crt_neighbourhood_2;
				for (std::vector<int>::iterator ni = prev_neighbours.begin(); ni != prev_neighbours.end(); ni++)
				{				
					float query_pt[3] = { m_pointList[*ni].m_vertex.Position[0], m_pointList[*ni].m_vertex.Position[1], m_pointList[*ni].m_vertex.Position[2]};				
					resultSet_1.init(&ret_index[0], &out_dist_sqr[0] );
					index.findNeighbors(resultSet_1, &query_pt[0], nanoflann::SearchParams(10));
					edge_neighbours = 0;
					for (int j=0; j<num_results*3; j++)
					{
						if (m_pointList[ret_index[j]].m_typeId==corner_seed_id && (edge_visited.find(ret_index[j]) == edge_visited.end()))
						{
							current_neighbours.push_back(ret_index[j]);
							edge_visited[ret_index[j]] = true;
							show.push_back(ret_index[j]);
							crt_neighbourhood_2.push_back(ret_index[j]);
							edge_points_not_visited--;
						}
						if (edge_neighbours == num_results) break;
					}
				}

				if (current_neighbours.size() == 0) 
				{
					//clear neighbourhood vector
					neighbourhoods.clear();
					if (debug) std::cout << "[End of Edge] Moving somewhere else ... " << std::endl;
					break; //break out of this while loop ... we're at the end of an edge
				} else {
					neighbourhoods.push_back(std::pair<float,std::vector<int>>(crt_radius,crt_neighbourhood_2));
				}

				//std::cout << "Showing Next Group of Neighbours " << std::endl;
				if (p_view) SetVisiblePointsWithIds(show);
			}
			else 
			{
				//sphere is bigger ... corner points ??? hope so !!
				//we should take in consideration all edge points now.

				if (debug) std::cout<<"[Corner Detected] Roll Count=" << roll_count << " SR=" << sphere_radius << ", this SR=" << new_sphere_radius << std::endl;
				corners_detected++;
				for (std::vector<std::pair<float,std::vector<int>>>::iterator ni = neighbourhoods.begin(); ni != neighbourhoods.end(); ni++)
				{
					float radius = ni->first;
					std::vector<int> n = ni->second;
					for (std::vector<int>::iterator nii = n.begin(); nii != n.end(); nii++)
					{
						m_pointList[*nii].m_typeId = 5;
						corner_points_count++;
					}
				}
				neighbourhoods.clear();				
				prev_neighbours.clear();
				current_neighbours.clear();
			}
		}
	}

	//Finally convert the seeds back to edges (unless they were turned to corner points)
	for (int i=0; i<seeds.size(); i++)
	{
		if (m_pointList[seeds[i]].m_typeId == 6) m_pointList[seeds[i]].m_typeId = 3;
		m_pointList[seeds[i]].m_vertex.Position -= shift;
	}

	std::cout << "[KDTreePointCloud] Corners Detected = " << corners_detected << ", New Corner Points =" << corner_points_count << std::endl;		
	return true;
}

//----------------------------------------------------------------------------------------------
// Method for creating an instance of the KDPointCloud
//----------------------------------------------------------------------------------------------
boost::shared_ptr<IPointCloud> KDTreePointCloud::CreateInstance(void) {
	return boost::shared_ptr<IPointCloud>(new KDTreePointCloud(0.02f));
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::IsBounded(void) const
{
	return true;
}
//----------------------------------------------------------------------------------------------
void KDTreePointCloud::ComputeArea(void)
{
	m_fArea = 0.f;
	
	/*for (int nIdx = 0; nIdx < m_nTriangleCount; ++nIdx, ++pTriangle)
	{
		const Vertex	&v0 = m_pVertexList[pTriangle->VertexID[0]],
						&v1 = m_pVertexList[pTriangle->VertexID[1]],
						&v2 = m_pVertexList[pTriangle->VertexID[2]];

		m_fArea += Vector3::Cross(v1.Position - v0.Position, v2.Position - v0.Position).Length() * 0.5f;
	}*/
}
//----------------------------------------------------------------------------------------------
float KDTreePointCloud::GetArea(void) const
{
	return m_fArea;
}
//----------------------------------------------------------------------------------------------

bool KDTreePointCloud::Intersects(const Ray &p_ray, DifferentialSurface &p_surface)
{	
	// Compute initial parametric range of ray inside kd-tree extent
	Ray ray(p_ray);
	float tmin, tmax;
	if (!m_rootNode.BoundingBox.Intersects(p_ray, tmin, tmax))
	{
		return false;
	}

	// Prepare to traverse kd-tree for ray	
	Vector3 invDir(p_ray.DirectionInverseCache);
	
#define MAX_TODO 64
	KDTreeStackElement todo[MAX_TODO];
	int todoPos = 0;

	// Traverse kd-tree nodes in order for ray
	bool hit = false;
	const KDTreePointCloudNode *node = &m_rootNode;
	while (node != NULL) {
		// Bail out if we found a hit closer than the current node
		if (ray.Max < tmin) break;
		if (node->Type == Internal) {
			// Process kd-tree interior node

			// Compute parametric distance along ray to split plane
			int axis = node->Axis;
			float tplane = (node->Partition - ray.Origin[axis]) * invDir[axis];

			// Get node children pointers for ray
			KDTreePointCloudNode *firstChild, *secondChild;
			int belowFirst = (ray.Origin[axis] <  node->Partition) ||
							 (ray.Origin[axis] == node->Partition && ray.Direction[axis] <= 0.f);
			if (belowFirst) {
				firstChild = node->m_pChild[0];
				secondChild = node->m_pChild[1];
			}
			else {
				firstChild = node->m_pChild[1];
				secondChild = node->m_pChild[0];
			}

			// Advance to next child node, possibly enqueue other child
			if (tplane > tmax || tplane <= 0)
				node = firstChild;
			else if (tplane < tmin)
				node = secondChild;
			else {
				// Enqueue _secondChild_ in todo list
				todo[todoPos].pNode = secondChild;
				todo[todoPos].Min = tplane;
				todo[todoPos].Max = tmax;
				++todoPos;
				node = firstChild;
				tmax = tplane;
			}
		}
		else {
			for (size_t n = 0, count = node->PointList.Size(); n < count; n++)
			{			
				if (m_pointList[node->PointList[n]].m_visible)    //Only check for an intersection when this point is visible
				if (IntersectSphere(ray.Origin, ray.Direction, m_pointList[node->PointList[n]], ray.Min, ray.Max, p_surface))
				//if (IntersectPlane(ray.Origin, ray.Direction, m_pointList[node->PointList[n]].Position, ray.Min, ray.Max, p_surface))				
				{
					hit = true;
					if (p_surface.Distance < ray.Max) p_surface.GeometryFragment.SetDataEx(node->PointList[n]);
					ray.Max = Maths::Min(ray.Max, p_surface.Distance);					
				}
			}

			// Grab next node to process from todo list
			if (todoPos > 0) {
				--todoPos;
				node = todo[todoPos].pNode;
				tmin = todo[todoPos].Min;
				tmax = todo[todoPos].Max;
			}
			else
				break;
		}
	}	
	return hit;
}

//----------------------------------------------------------------------------------------------
// Performs a quick intersection test on the kd-tree, returning as soon as an intersection has occurred
// , but providing no further details as to the intersection itself.
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::Intersects(const Ray &p_ray)
{	
	// Compute initial parametric range of ray inside kd-tree extent
	Ray ray(p_ray);
	float tmin, tmax;
	if (!m_rootNode.BoundingBox.Intersects(p_ray, tmin, tmax))
	{
		return false;
	}

	// Prepare to traverse kd-tree for ray	
	Vector3 invDir(p_ray.DirectionInverseCache);
	
#define MAX_TODO 64
	KDTreeStackElement todo[MAX_TODO];
	int todoPos = 0;

	// Traverse kd-tree nodes in order for ray
	bool hit = false;
	const KDTreePointCloudNode *node = &m_rootNode;
	while (node != NULL) {
		// Bail out if we found a hit closer than the current node
		if (ray.Max < tmin) break;
		if (node->Type == Internal) {
			// Process kd-tree interior node

			// Compute parametric distance along ray to split plane
			int axis = node->Axis;
			float tplane = (node->Partition - ray.Origin[axis]) * invDir[axis];

			// Get node children pointers for ray
			KDTreePointCloudNode *firstChild, *secondChild;
			int belowFirst = (ray.Origin[axis] <  node->Partition) ||
							 (ray.Origin[axis] == node->Partition && ray.Direction[axis] <= 0.f);
			if (belowFirst) {
				firstChild = node->m_pChild[0];
				secondChild = node->m_pChild[1];
			}
			else {
				firstChild = node->m_pChild[1];
				secondChild = node->m_pChild[0];
			}

			// Advance to next child node, possibly enqueue other child
			if (tplane > tmax || tplane <= 0)
				node = firstChild;
			else if (tplane < tmin)
				node = secondChild;
			else {
				// Enqueue _secondChild_ in todo list
				todo[todoPos].pNode = secondChild;
				todo[todoPos].Min = tplane;
				todo[todoPos].Max = tmax;
				++todoPos;
				node = firstChild;
				tmax = tplane;
			}
		}
		else {
			for (size_t n = 0, count = node->PointList.Size(); n < count; n++)
			{				
				if (m_pointList[node->PointList[n]].m_visible)    //Only check for an intersection when this point is visible
				if (IntersectSphere(ray.Origin, ray.Direction, m_pointList[node->PointList[n]].m_vertex.Position, ray.Min, ray.Max))
				{
					return true;
				}
			}

			// Grab next node to process from todo list
			if (todoPos > 0) {
				--todoPos;
				node = todo[todoPos].pNode;
				tmin = todo[todoPos].Min;
				tmax = todo[todoPos].Max;
			}
			else
				break;
		}
	}	
	return hit;
}

//----------------------------------------------------------------------------------------------
// Performs a quick intersection test, returning whether an intersection has occurred
// or not, but providing no further details as to the intersection itself.
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::IntersectsTrivial(const Ray &p_ray)
{	
	int count = m_pointList.size();	
	for (int i=0; i<count; i++)
	{		
		if (IntersectSphere(p_ray.Origin, p_ray.Direction, m_pointList[i].m_vertex.Position, p_ray.Min, p_ray.Max))
		{
			return true;
		}
	}
	return false;
}

//----------------------------------------------------------------------------------------------
// Returns the result of an intersection between a ray and the kD-Tree. The method also
// populates a DifferentialSurface structure with all the details of the intersected
// surface.
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::IntersectsTrivial(const Ray &p_ray, DifferentialSurface &p_surface)
{	
	//Simple iteration over all points
	bool intersects = false;		
	float t = p_ray.Max;

	int count = m_pointList.size();
	for (int i=0; i<count; i++)
	{
		if (IntersectSphere(p_ray.Origin, p_ray.Direction, m_pointList[i], p_ray.Min, t, p_surface))
		{
			t = Maths::Min(p_surface.Distance, t);
			intersects = true;
		}
	}
	return intersects;
}

//----------------------------------------------------------------------------------------------
// IntersectSphere and set differential surface
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::IntersectSphere(const Vector3 &p_rOrigin, const Vector3 &p_rDirection, CloudPoint &p_cloudPoint, float p_rMin, float p_rMax, DifferentialSurface &p_surface)
{
	const Vector3 &temp = p_rOrigin - p_cloudPoint.m_vertex.Position;
	
	double a = Vector3::Dot(p_rDirection, p_rDirection);
	double b = 2 * Vector3::Dot(p_rDirection, temp);	
	double c = Vector3::Dot(temp, temp) - m_fSphereRadiusSq;
	double discriminant = b*b - 4*a*c;
	
	if (discriminant > 0)
	{
		discriminant = Maths::Sqrt((float)discriminant);
		double t = (-b - discriminant) / (2 * a);
		
		if (t <= p_rMin)
			t = (-b + discriminant) / (2 * a);

		if (t <= p_rMin || t >= p_rMax)
			return false;

		// Populate differential surface
		p_surface.SetShape(this);
		p_surface.Distance = t;
		p_surface.Point = p_rOrigin + t * p_rDirection;

		p_surface.GeometryNormal = Vector3::Normalize(p_surface.Point - p_cloudPoint.m_vertex.Position);
		p_surface.ShadingNormal = p_surface.GeometryNormal;
		p_surface.GeometryFragment.SetGroupId(p_cloudPoint.m_segmentId % IPointCloud::GetNumberOfMaterials());		
		return true;
	}
	return false;
}

//----------------------------------------------------------------------------------------------
// Intersect Sphere quick check
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::IntersectSphere(const Vector3 &p_rOrigin, const Vector3 &p_rDirection, Vector3 &p_sCentre, float p_rMin, float p_rMax)
{
	const Vector3 &temp = p_rOrigin - p_sCentre;
	
	double a = Vector3::Dot(p_rDirection, p_rDirection);
	double b = 2 * Vector3::Dot(p_rDirection, temp);
	double c = Vector3::Dot(temp, temp) - m_fSphereRadiusSq;

	double discriminant = b*b - 4*a*c;
	
	if (discriminant > 0)
	{
		discriminant = Maths::Sqrt((float)discriminant);
		double t = (-b - discriminant) / (2 * a);
		
		if (t <= p_rMin)
			t = (-b + discriminant) / (2 * a);

		if (t <= p_rMin || t >= p_rMax)
			return false;
		
		return true;
	}
	return false;
}

//----------------------------------------------------------------------------------------------
// Intersect Plane - for splatting
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::IntersectPlane(const Vector3 &p_rOrigin, const Vector3 &p_rDirection, Vector3 &p_sCentre, float p_rMin, float p_rMax, DifferentialSurface &p_surface) 
{
	//Vectpr3 p = p_sCentre;	
	//Vector3 n = -p_rDirection;
	//Vector3 w = p_rOrigin - p_sCentre;
	//Vector3 u = p_rDirection;
	float s = p_rDirection.Dot(p_rOrigin - p_sCentre) / -p_rDirection.Dot(p_rDirection);
	Vector3 intersectionPoint = p_rOrigin + s * p_rDirection;
	if (Vector3::DistanceSquared(intersectionPoint,p_sCentre) < m_fSphereRadiusSq)
	{
		// Populate differential surface
		//p_surface.SetShape(this);
		p_surface.Distance = s;
		p_surface.Point = intersectionPoint;

		p_surface.GeometryNormal = -p_rDirection;
		p_surface.ShadingNormal = p_surface.GeometryNormal;	
		return true;
	}
	return false;
}

//----------------------------------------------------------------------------------------------
// Intersect Plane - quick check
//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::IntersectPlane(const Vector3 &p_rOrigin, const Vector3 &p_rDirection, Vector3 &p_sCentre, float p_rMin, float p_rMax) 
{
	return true;
}

//-------------------------------------------
//Build KD-tree over points. Entry function.
//-------------------------------------------
void KDTreePointCloud::BuildHierarchy(KDTreePointCloudNode *p_pNode, std::vector<CloudPoint> &p_objectList, int p_nAxis, int p_nDepth)
{
	List<int> indices(m_pointList.size());
	for (int i = 0; i<m_pointList.size(); i++) indices.PushBack(i);
	ComputeBounds(indices, p_pNode->BoundingBox, 1e-4f, 1e-4f);
	//p_pNode is the root node of the kdtree
	BuildHierarchy_S2(p_pNode, indices, p_pNode->BoundingBox.GetExtent().ArgMaxAbsComponent(), 0);
}

//-------------------------------------------
//Recursively build kd-tree ..
//-------------------------------------------
void KDTreePointCloud::BuildHierarchy_S2(KDTreePointCloudNode *p_pNode, List<int> &p_objectList, int p_nAxis, int p_nDepth)
{			
	// Update stats
	m_statistics.m_maxTreeDepth = Maths::Min(p_nDepth, m_statistics.m_maxTreeDepth);

	// If we have enough objects, we consider this node a leaf
	if ((int)p_objectList.Size() <= m_nMaxLeafObjects || p_nDepth == m_nMaxTreeDepth)
	{
		//std::cout << "Adding leaf node [" << p_objectList.Size() << ", " << p_nDepth << "]" << std::endl;
		p_pNode->Type = Leaf; 
		p_pNode->PointList.PushBack(p_objectList);

		m_statistics.m_leafNodeCount++;
		m_statistics.m_minTreeDepth = Maths::Min(m_statistics.m_minTreeDepth, p_nDepth);
		m_statistics.m_minLeafPointCount = Maths::Min(m_statistics.m_minLeafPointCount, (int)p_objectList.Size());
		m_statistics.m_maxLeafPointCount = Maths::Min(m_statistics.m_maxLeafPointCount, (int)p_objectList.Size());
	}
	else
	{
		//std::cout << "Adding internal node [" << p_objectList.Size() << ", " << p_nDepth << "]" << std::endl;
		p_pNode->Type = Internal;
		p_pNode->Axis = p_nAxis;
		p_pNode->Partition = FindPartitionPlane(p_objectList, p_pNode->BoundingBox, p_nAxis, SpatialMedian);

		List<int> leftList, rightList;
		leftList.Clear(); rightList.Clear(); 

		p_pNode->m_pChild[0] = RequestNode();
		p_pNode->m_pChild[1] = RequestNode();

		AxisAlignedBoundingBox 
			&leftAABB = p_pNode->m_pChild[0]->BoundingBox,
			&rightAABB = p_pNode->m_pChild[1]->BoundingBox;

		leftAABB = p_pNode->BoundingBox;
		rightAABB = p_pNode->BoundingBox;

		leftAABB.SetMaxExtent(p_nAxis, p_pNode->Partition);
		rightAABB.SetMinExtent(p_nAxis, p_pNode->Partition);

		Distribute(p_objectList, p_pNode->Partition, p_pNode->Axis, leftList, rightList);

		int nAxis = (p_nAxis + 1) % 3,
			nDepth = p_nDepth + 1;

		BuildHierarchy_S2(p_pNode->m_pChild[0], leftList, nAxis, nDepth);
		BuildHierarchy_S2(p_pNode->m_pChild[1], rightList, nAxis, nDepth);

		m_statistics.m_internalNodeCount++;
	}
}

//----------------------------------------------------------------------------------------------
// Returns a literal with information on the acceleration structure.
//----------------------------------------------------------------------------------------------
std::string KDTreePointCloud::ToString(void) const
{
	return "KDTreePointCloud";
}

//----------------------------------------------------------------------------------------------
float KDTreePointCloud::GetPdf(const Vector3 &p_point) const { return 1.f / m_fArea; }
//----------------------------------------------------------------------------------------------
Vector3 KDTreePointCloud::SamplePoint(float p_u, float p_v, Vector3 &p_normal)
{
	return Vector3::Zero;
}
//----------------------------------------------------------------------------------------------
Vector3 KDTreePointCloud::SamplePoint(const Vector3 &p_viewPoint, float p_u, float p_v, Vector3 &p_normal)
{
	return Vector3::Zero;
}
//----------------------------------------------------------------------------------------------
//Persist KDTreePointCloud with segmentation information to file
bool KDTreePointCloud::WriteToBKD3File(std::string p_filePath, bool p_blab, MaterialGroup * p_materialGroup)
{
	std::string pcsPath = p_filePath+".bkd3";
	const char * filename = pcsPath.c_str();
	size_t pc_size = m_pointList.size();
	std::filebuf fbuf;
	fbuf.open(filename, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);
	float size_of_record = (3*sizeof(float)) + sizeof(int) + sizeof(int) + sizeof(int) + sizeof(char) + sizeof(char);
	fbuf.pubseekoff( (pc_size*size_of_record) + (16*sizeof(float)) - sizeof(char), std::ios_base::beg);
	fbuf.sputc(0);
	fbuf.close();

	boost::interprocess::file_mapping m_file(filename, boost::interprocess::read_write);
	boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);						    

	float * r_addr_f = (float*)region.get_address();			
	char * r_addr_c;
	size_t r_size = region.get_size();				
	std::cout << "[KDTreePointCloud      ] Writing to Binary File " << p_filePath << ". Size= " << r_size << std::endl;
	size_t idx = 0;
	
	*r_addr_f = pc_size; r_addr_f++;
	*r_addr_f = GetWorldType(); r_addr_f++;
	*r_addr_f = GetkNNType(); r_addr_f++;
	*r_addr_f = GetkNNFlood(); r_addr_f++;
	*r_addr_f = GetkNNTypeMode(); r_addr_f++;
	*r_addr_f = GetkNNFloodMode(); r_addr_f++;
	*r_addr_f = GetEdgeCornerDetect(); r_addr_f++;
	*r_addr_f = GetSurfaceCurvature(); r_addr_f++;
	*r_addr_f = GetCornerRatioEpsilon(); r_addr_f++;
	*r_addr_f = GetNoise(); r_addr_f++;
	*r_addr_f = GetBallRadius(); r_addr_f++;
	*r_addr_f = GetkNNTypeRadius(); r_addr_f++;
	*r_addr_f = GetkNNFloodRadius(); r_addr_f++;
	*r_addr_f = GetMinDist(); r_addr_f++;
	*r_addr_f = GetMaxDist(); r_addr_f++;
	*r_addr_f = GetRadius(); r_addr_f++;

	/*std::cout << "Writing:" << GetWorldType() << " kNNType:" << GetkNNType() << " kNNFlood:" << GetkNNFlood() << " kNNTypeMode:" << GetkNNTypeMode() << " kNNFloodMode:" << GetkNNFloodMode() 
				<< " ECD:" << GetEdgeCornerDetect() << " Curvature:" << GetSurfaceCurvature() 
				<< " CREpsilon:" << GetCornerRatioEpsilon() << " Noise:" << GetNoise() << " BallRadius:" << GetBallRadius() << " knnTypeRadius:" << GetkNNTypeRadius() << " kNNFloodRadius:" << GetkNNFloodRadius() 
				<< " MinDist:" << GetMinDist() << " MaxDist:" << GetMaxDist() << " Radius:" << GetRadius() << std::endl;
				*/
	for (int i = 0; i<pc_size-1; i++) {					
		*r_addr_f = m_pointList[i].m_vertex.Position.X; r_addr_f++;
		*r_addr_f = m_pointList[i].m_vertex.Position.Y; r_addr_f++;
		*r_addr_f = m_pointList[i].m_vertex.Position.Z; r_addr_f++;
		*r_addr_f = m_pointList[i].m_groupId; r_addr_f++;
		*r_addr_f = m_pointList[i].m_segmentId; r_addr_f++;
		*r_addr_f = m_pointList[i].m_segmentId2; r_addr_f++;
		r_addr_c = (char*)r_addr_f;
		*r_addr_c = m_pointList[i].m_typeId; r_addr_c++;
		*r_addr_c = m_pointList[i].m_seed; r_addr_c++;
		r_addr_f = (float*)r_addr_c;		
	}

	region.flush();
	
	if (p_blab)
	{
		//write binary labelled file as well in order to open using the pcdviewer tool
		std::string blabPath = p_filePath+".blab";
		std::ofstream output(blabPath.c_str(), std::fstream::binary) ;
		if(output.fail()) {
			std::cout << "[KDTreePointCloud BLAB] Failed to open file " << blabPath << std::endl;
			return false;
		}

		int float_size = sizeof(float);
		int int_size = sizeof(int);			
		int char_size = sizeof(char);

		int num = m_pointList.size();
		output.write((char*)&num, int_size);

		float defaultNormal_X = 0.f;
		float defaultNormal_Y = 0.f;
		float defaultNormal_Z = 0.f;
		int materialIndex = 1;

		std::map<int, std::vector<int>> segments;

		Spectrum f;
		for (int i=0; i<num; ++i) {			
			materialIndex = m_pointList[i].m_segmentId % IPointCloud::GetNumberOfMaterials();
			f = p_materialGroup->GetByIndex(materialIndex)->SampleTexture(DifferentialSurface(), 0);
			RGBPixel p(f[0], f[1], f[2]);
			float values[10] = {m_pointList[i].m_vertex.Position.X, m_pointList[i].m_vertex.Position.Y, m_pointList[i].m_vertex.Position.Z, 
				defaultNormal_X, defaultNormal_Y, defaultNormal_Z, p.R, p.G, p.B, 0.f };
			segments[m_pointList[i].m_segmentId].push_back(i);
			output.write((char*)values, 10 * float_size);
		}
		
		//Write number of groups
		int num_groups = segments.size();
		output.write((char*)&num_groups, int_size); 

		float alpha = 0.f;
		std::vector<int> segmentIndices;	
		std::string str;
		for (std::map<int, std::vector<int>>::iterator segmentsIterator = segments.begin(); segmentsIterator != segments.end(); segmentsIterator++) {
			segmentIndices = segmentsIterator->second;
			//Write label
			str = "unknown_"+boost::lexical_cast<std::string, int>(m_pointList[segmentIndices[0]].m_segmentId);
			if (m_pointList[segmentIndices[0]].m_typeId==4) str = "surface_"+boost::lexical_cast<std::string, int>(m_pointList[segmentIndices[0]].m_segmentId);
			if (m_pointList[segmentIndices[0]].m_typeId==3) str = "edge_"+boost::lexical_cast<std::string, int>(m_pointList[segmentIndices[0]].m_segmentId);
			int num_char = str.size();
			output.write((char*)&num_char, int_size);
			for (int i=0; i<num_char; ++i) {
				char c = str[i];
				if (c == ' ') c = '-';
				output.write(&c, char_size);
			}
			//End label writing

			materialIndex = m_pointList[segmentIndices[0]].m_segmentId % IPointCloud::GetNumberOfMaterials();
			f = p_materialGroup->GetByIndex(materialIndex)->SampleTexture(DifferentialSurface(), 0);
			RGBPixel p(f[0], f[1], f[2]);	
			output.write((char*)&p.R, float_size);
			output.write((char*)&p.G, float_size);
			output.write((char*)&p.B, float_size);
			output.write((char*)&alpha, float_size);

			int num_point = segmentIndices.size();
			output.write((char*)&num_point, int_size);
			
			int idx = 0;
			for (unsigned int j=0; j<segmentIndices.size(); ++j) {
				idx = segmentIndices[j];
				output.write((char*)&idx, int_size);
			}

			int chld_num = 0;
			output.write((char*)&chld_num, int_size);
		}

	}

	return true;
}

//----------------------------------------------------------------------------------------------
bool KDTreePointCloud::ReadFromBKD3File(std::string p_filePath)
{
	// Use filename as the default object name
	//boost::filesystem::path pcsPath(p_filePath);	
		
	// Open .pcs file
	std::string pcsPath = p_filePath+".bkd3";
	boost::filesystem::path p (pcsPath, boost::filesystem::native);
	if ( !boost::filesystem::exists( p ) )
	{
		std::cerr << "Error : Couldn't open file '" << pcsPath << "'" << std::endl;
		return false;
	}
		
	//we assume all points fit in main memory
	boost::interprocess::file_mapping m_file;		
	boost::interprocess::mapped_region *m_region;	
	float * r_addr_f;
	char * r_addr_c;
	
	const char * filename = pcsPath.c_str();
	m_file = boost::interprocess::file_mapping(filename, boost::interprocess::read_only);
	m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only);					
	r_addr_f = (float*)m_region->get_address();
	size_t r_size = m_region->get_size();					
	size_t p_size = (int)*r_addr_f; r_addr_f++;

	std::cout << "[KDTreePointCloud] Reading from Binary File " << pcsPath << ". Number of Points = " << p_size << std::endl;

	VertexP nxtvrtx;	
		
	SetWorldType((int)*r_addr_f); r_addr_f++;
	SetkNNType((int)*r_addr_f); r_addr_f++;
	SetkNNFlood((int)*r_addr_f); r_addr_f++;
	SetkNNTypeMode((int)*r_addr_f); r_addr_f++;
	SetkNNFloodMode((int)*r_addr_f); r_addr_f++;
	SetEdgeCornerDetect((int)*r_addr_f); r_addr_f++;
	SetSurfaceCurvature(*r_addr_f); r_addr_f++;
	SetCornerRatioEpsilon(*r_addr_f); r_addr_f++;
	SetNoise(*r_addr_f); r_addr_f++;
	SetBallRadius(*r_addr_f); r_addr_f++;
	SetkNNTypeRadius(*r_addr_f); r_addr_f++;
	SetkNNFloodRadius(*r_addr_f); r_addr_f++;
	SetMinDist(*r_addr_f); r_addr_f++;
	SetMaxDist(*r_addr_f); r_addr_f++;  	
	SetRadius(*r_addr_f); r_addr_f++;	

	/*std::cout << "Reading:" << GetWorldType() << " kNNType:" << GetkNNType() << " kNNFlood:" << GetkNNFlood() << " kNNTypeMode:" << GetkNNTypeMode() << " kNNFloodMode:" << GetkNNFloodMode() 
				<< " ECD:" << GetEdgeCornerDetect() << " Curvature:" << GetSurfaceCurvature() 
				<< " CREpsilon:" << GetCornerRatioEpsilon() << " Noise:" << GetNoise() << " BallRadius:" << GetBallRadius() << " knnTypeRadius:" << GetkNNTypeRadius() << " kNNFloodRadius:" << GetkNNFloodRadius() 
				<< " MinDist:" << GetMinDist() << " MaxDist:" << GetMaxDist() << " Radius:" << GetRadius() << std::endl;
	*/
	float x, y, z;
	int groupID, segmentID, segmentID2;
	char typeID, seed;

	for (int i = 0; i<p_size; i++) 
	{		
		x = *r_addr_f; r_addr_f++;
		y = *r_addr_f; r_addr_f++;
		z = *r_addr_f; r_addr_f++;
		nxtvrtx.Position.Set(x, y, z);		
		groupID = *r_addr_f; r_addr_f++;
		segmentID = *r_addr_f; r_addr_f++;
		segmentID2 = *r_addr_f; r_addr_f++;
		r_addr_c = (char*) r_addr_f;
		typeID = *r_addr_c; r_addr_c++;
		seed = *r_addr_c; r_addr_c++;
		r_addr_f = (float*)r_addr_c;
		AddPoint(CloudPoint(nxtvrtx, groupID, typeID, segmentID, segmentID2));
	}

	std::cout << "[KDTreePointCloud] Reading from Binary File. Done." << std::endl;
	return true;	
}


